/*************************************************************
 * @file motion_planner.c
 * @author MicroOrange
 * @brief 运动规划器层 反馈与速度
 * @date 2022.2.23
 *************************************************************/

#include "tim.h"
#include "usart.h"
#include "gpio.h"
#include "string.h"
#include "stdio.h"
#include "pid.h"
#include "motor.h"
#include "motor.h"
#include "fish_controller.h"
#include "JY901.h"
#include "cpg_controller.h"
#include "speed_convert_layer_v2.h"
#include "math.h"
#include "para_service.h"
#include "motion_planner.h"

uint8_t motion_mode = MOTION_MODE_REMOTE;

pid_type pid_rotate_yaw;
pid_type pid_rotate_pitch;
pid_type pid_rotate_roll;

double test1_freq = 1;

double motion_threedimrotate_target_y=0;
double motion_threedimrotate_target_p=0;
double motion_threedimrotate_target_r=0;

double motion_test2_vf = 0;
double motion_test2_vd = 0;
double motion_test2_vr = 0;
double motion_test2_vp = 0;
double motion_test2_vy = 0;

double channel_value[10];


void Motion_Init(){
    PID_init(&pid_rotate_yaw, PID_POSITION, 0.00, 0, 0, 2, 3);
    PID_init(&pid_rotate_pitch, PID_POSITION, -3, 0, 0, 50, 10);
    PID_init(&pid_rotate_roll, PID_POSITION, 0.00, 0, 0, 20, 3);
}

/**
 * 三轴PID稳定
 */
void Motion_ThreeDimRotate(double deg_y, double deg_p, double deg_r, fish_t *remora){
    double now_y = IMU_GetYaw();
    double now_p = IMU_GetPitch();
    double now_r = IMU_GetRoll();

//    printf("%d %d",(int)now_r, (int)motion_threedimrotate_target_r);
    PID_calc(&pid_rotate_yaw,now_y, deg_y);
    PID_calc(&pid_rotate_pitch, now_p, deg_p);
    PID_calc(&pid_rotate_roll, now_r, deg_r);
    double speed_p = pid_rotate_pitch.out;
    ConvertLayer_Update(remora, motion_test2_vf, 0, 0, 0, speed_p);
}

void Motion_SetThreeDimRotate(double target_y, double target_p, double target_r){
    motion_mode = MOTION_MODE_THREEDIMROTATE;
    motion_threedimrotate_target_y = target_y;
    motion_threedimrotate_target_p = target_p;
    motion_threedimrotate_target_r = target_r;
}

void Motion_ParaThreeDimRotate(int para_index, double value){
    switch (para_index) {
        case PARA_MOTION_THREEDIMROTATE_PID_R_KP:
            pid_rotate_roll.Kp = value;
            break;
        case PARA_MOTION_THREEDIMROTATE_PID_R_KI:
            pid_rotate_roll.Ki = value;
            break;
        case PARA_MOTION_THREEDIMROTATE_PID_R_KD:
            pid_rotate_roll.Kd = value;
            break;
        case PARA_MOTION_THREEDIMROTATE_PID_P_KP:
            pid_rotate_pitch.Kp = value;
            break;
        case PARA_MOTION_THREEDIMROTATE_PID_P_KI:
            pid_rotate_pitch.Ki = value;
            break;
        case PARA_MOTION_THREEDIMROTATE_PID_P_KD:
            pid_rotate_pitch.Kd = value;
            break;
        case PARA_MOTION_THREEDIMROTATE_PID_Y_KP:
            pid_rotate_yaw.Kp = value;
            break;
        case PARA_MOTION_THREEDIMROTATE_PID_Y_KI:
            pid_rotate_yaw.Ki = value;
            break;
        case PARA_MOTION_THREEDIMROTATE_PID_Y_KD:
            pid_rotate_yaw.Kd = value;
            break;
        case PARA_MOTION_THREEDIMROTATE_R:
            motion_threedimrotate_target_r = value;
            break;
        case PARA_MOTION_THREEDIMROTATE_P:
            motion_threedimrotate_target_p = value;
            break;
        case PARA_MOTION_THREEDIMROTATE_Y:
            motion_threedimrotate_target_y = value;
            break;

        default:
            break;
    }
}

/**
 *
 * @param remora
 */

void Motion_SetStop(fish_t *remora){
    motion_mode = MOTION_MODE_STOP;
    remora->bumper_neck_pitch.output = 0;
    remora->bumper_disc.output = 0;

    ConvertLayer_Update(remora, 0, 0, 0, 0, 0);
}

void Motion_SetTest1(fish_t *remora){
    motion_mode = MOTION_MODE_TEST1;

    remora->cpg_left_pectrol.amplitude = 0;
    remora->cpg_left_pectrol.offset = 0;
    remora->cpg_left_pectrol.frequency = test1_freq;
    remora->cpg_right_pectrol.amplitude = 0;
    remora->cpg_right_pectrol.offset = 0;
    remora->cpg_right_pectrol.frequency = test1_freq;
    remora->cpg_tail1.amplitude = 0;
    remora->cpg_tail1.offset = 0;
    remora->cpg_tail1.frequency = 0.8;
    remora->cpg_tail2.amplitude = 0;
    remora->cpg_tail2.offset = 0;
    remora->cpg_tail2.frequency = 0.8;
    remora->bumper_neck_pitch.speed = 5;
}

void Motion_SetTest2(fish_t *remora, double Vf, double Vd, double Vr, double Vp, double Vy){
    ConvertLayer_Update(remora, Vf, Vd, Vy, Vr, Vp);
}

void Motion_ParaTest2(int para_index, double value){
    switch (para_index) {
        case PARA_MOTION_MODE_TEST2_Vf:
            motion_test2_vf = value;
            break;
        case PARA_MOTION_MODE_TEST2_Vd:
            motion_test2_vd = value;
            break;
        case PARA_MOTION_MODE_TEST2_Vr:
            motion_test2_vr = value;
            break;
        case PARA_MOTION_MODE_TEST2_Vp:
            motion_test2_vp = value;
            break;
        case PARA_MOTION_MODE_TEST2_Vy:
            motion_test2_vy = value;
            break;
        case PARA_REMOTE_CHANNEL_1:
            channel_value[0] = value;
            break;
        case PARA_REMOTE_CHANNEL_2:
            channel_value[1] = value;
            break;
        case PARA_REMOTE_CHANNEL_3:
            channel_value[2] = value;
            break;
        case PARA_REMOTE_CHANNEL_4:
            channel_value[3] = value;
            break;
        case PARA_REMOTE_CHANNEL_5:
            channel_value[4] = value;
            break;
        case PARA_REMOTE_CHANNEL_6:
            channel_value[5] = value;
            break;
        case PARA_REMOTE_CHANNEL_7:
            channel_value[6] = value;
            break;
        case PARA_REMOTE_CHANNEL_8:
            channel_value[7] = value;
            break;
        default:
            break;
    }
}

void Motion_TestSetFreq(double freq){
    test1_freq = freq;
}

void Motion_SetModeSimple(int mode){
    motion_mode = mode;
}

void Motion_NeckPitch(fish_t *remora){
    remora->bumper_disc.mode = BUMPER_MODE_POS;
    remora->bumper_neck_pitch.mode = BUMPER_MODE_POS;
    remora->bumper_disc.set = 0;
    remora->bumper_neck_pitch.set = 0;
    double l_amp = motion_test2_vf;
    double l_ang = motion_test2_vd;
    double r_amp = motion_test2_vr;
    double r_ang = motion_test2_vp;
    double freq = motion_test2_vy;
    /**
     * 直接操作CPG层 仅供运动形式DEBUG
     */
        remora->cpg_left_pectrol.frequency = freq;


        remora->cpg_right_pectrol.frequency = freq ;


    remora->cpg_left_pectrol.amplitude = l_amp;
    remora->cpg_left_pectrol.offset = l_ang;
    remora->cpg_right_pectrol.amplitude = r_amp;
    remora->cpg_right_pectrol.offset = r_ang;

    double now_p = IMU_GetPitch();
    PID_calc(&pid_rotate_pitch, now_p, motion_threedimrotate_target_p);
    remora->bumper_neck_pitch.speed = pid_rotate_pitch.out;

}

void Motion_PidRoll(fish_t *remora){

    remora->cpg_left_pectrol.frequency = 2;
    remora->cpg_right_pectrol.frequency = 2;
    double now_roll = IMU_GetRoll();
    PID_calc(&pid_rotate_roll, now_roll, motion_threedimrotate_target_r);
    double speed = pid_rotate_roll.out;
    if (speed>0){
        remora->cpg_left_pectrol.offset = -80;
        remora->cpg_right_pectrol.offset = 80;
        remora->cpg_left_pectrol.amplitude = fabs(speed);
        remora->cpg_right_pectrol.amplitude = fabs(speed);
    }
    if (speed<0){
        remora->cpg_left_pectrol.offset = 80;
        remora->cpg_right_pectrol.offset = -80;
        remora->cpg_left_pectrol.amplitude = fabs(speed);
        remora->cpg_right_pectrol.amplitude = fabs(speed);
    }
}

void Motion_DetachStep1(fish_t *remora){
    remora->bumper_disc.mode = BUMPER_MODE_SPEED;
    remora->bumper_disc.speed = 50;
}

void Motion_DetachStep2(fish_t *remora){
    remora->bumper_neck_pitch.mode = BUMPER_MODE_SPEED;
    remora->bumper_neck_pitch.speed = 30;
}

void Motion_Front(fish_t *remora) {
    remora->cpg_left_pectrol.offset = 0;
    remora->cpg_right_pectrol.offset = 0;

    if (motion_test2_vf < 1e-2) {
        remora->cpg_left_pectrol.amplitude = 0;
        remora->cpg_right_pectrol.amplitude = 0;
    } else {
        remora->cpg_left_pectrol.amplitude = motion_test2_vf;
        remora->cpg_right_pectrol.amplitude = motion_test2_vf;
        remora->cpg_left_pectrol.frequency = motion_test2_vd;
        remora->cpg_right_pectrol.frequency = motion_test2_vd;
    }
}

void Motion_Front_Pitch(fish_t *remora){
    if (motion_test2_vf < 1e-2){
        remora->cpg_tail1.amplitude = 0;
        remora->cpg_tail2.amplitude = 0;
    } else{
        remora->cpg_tail1.amplitude = 15.0;
        remora->cpg_tail2.amplitude = 20.0;
    }
    remora->cpg_tail1.offset = 0;
    remora->cpg_tail2.offset = 0;
    remora->cpg_tail1.frequency = motion_test2_vf;
    remora->cpg_tail2.frequency = motion_test2_vf;
    remora->cpg_left_pectrol.amplitude = 40;
    remora->cpg_right_pectrol.amplitude = 40;
    remora->cpg_left_pectrol.frequency = 2.5;
    remora->cpg_right_pectrol.frequency = 2.5;
    remora->cpg_left_pectrol.offset = 75;
    remora->cpg_right_pectrol.offset = 75;
}

void Motion_Remote_Control(fish_t *remora){
    double neck_pitch = channel_value[0];
    double pec = channel_value[1];
    double front = channel_value[2];
    double yaw = channel_value[3];
//    int neck_mode = lround(channel_value[4]);
    int neck_mode = -1;
    double pitch_ang = lround(channel_value[5]);
    int pec_mode = lround(channel_value[6]);
    int suck_mode = lround(channel_value[7]);

    double f_pec = fabs(pec);
    double pec_pitch_amp = 0.6;
    double pec_pitch_freq = 0.03;
    double pec_roll_amp = 0.6;
    double pec_roll_freq = 0.04;
    double tail_freq = 0.015;
    double tail_offset = 0.05;
    if (fabs(front) < 5){
        remora->cpg_tail1.amplitude = 0;
        remora->cpg_tail2.amplitude = 0;
        remora->cpg_tail1.offset = 0;
        remora->cpg_tail2.offset = 0;

    } else{
        remora->cpg_tail1.amplitude = 10.0;
        remora->cpg_tail2.amplitude = 12.0;
//        remora->cpg_tail1.offset = yaw * tail_offset;
//        remora->cpg_tail2.offset = yaw * tail_offset*1.33;
        remora->cpg_tail1.offset = yaw * 0;
        remora->cpg_tail2.offset = yaw * 0;
    }
    remora->cpg_tail1.frequency = front*tail_freq;
    remora->cpg_tail2.frequency = front*tail_freq;
    Motor_setPositionDegree(&remora->motor_neck_roll, yaw*2.7-135);


    if (pec_mode == 0){
        if (fabs(pec) > 5){
            if (pec>0){
                remora->cpg_left_pectrol.offset = -65;
                remora->cpg_right_pectrol.offset = -65;
            } else{
                remora->cpg_left_pectrol.offset = 50;
                remora->cpg_right_pectrol.offset = 50;
            }
            remora->cpg_left_pectrol.amplitude = f_pec*pec_pitch_amp;
            remora->cpg_right_pectrol.amplitude = f_pec*pec_pitch_amp;
            remora->cpg_left_pectrol.frequency = f_pec*pec_pitch_freq;
            remora->cpg_right_pectrol.frequency = f_pec*pec_pitch_freq;
        } else{
            remora->cpg_left_pectrol.offset = 0;
            remora->cpg_right_pectrol.offset = 0;
            remora->cpg_left_pectrol.amplitude = 0;
            remora->cpg_right_pectrol.amplitude = 0;
            remora->cpg_left_pectrol.frequency = 0;
            remora->cpg_right_pectrol.frequency = 0;
        }
    } else{
        if (fabs(pec) > 5){
            remora->cpg_left_pectrol.offset = 0;
            remora->cpg_right_pectrol.offset = 0;
            remora->cpg_left_pectrol.amplitude = f_pec*pec_roll_amp;
            remora->cpg_right_pectrol.amplitude = f_pec*pec_roll_amp;
            remora->cpg_left_pectrol.frequency = f_pec*pec_roll_freq;
            remora->cpg_right_pectrol.frequency = f_pec*pec_roll_freq;
        } else{
            remora->cpg_left_pectrol.offset = 0;
            remora->cpg_right_pectrol.offset = 0;
            remora->cpg_left_pectrol.amplitude = 0;
            remora->cpg_right_pectrol.amplitude = 0;
            remora->cpg_left_pectrol.frequency = 0;
            remora->cpg_right_pectrol.frequency = 0;
        }
    }

    if (suck_mode == 0){
        remora->bumper_disc.mode = BUMPER_MODE_SPEED;
        remora->bumper_disc.speed = 200;
    } else{
        remora->bumper_disc.mode = BUMPER_MODE_POS;
        remora->bumper_disc.set = -100;
    }

    if (neck_mode == 0){
        remora->bumper_neck_pitch.mode = BUMPER_MODE_POS;
        remora->bumper_neck_pitch.set = 0;
    } else if (neck_mode == -1){
        remora->bumper_neck_pitch.mode = BUMPER_MODE_POS;
        remora->bumper_neck_pitch.set = neck_pitch * 2.7 -135;
    } else if (neck_mode == 1){
        remora->bumper_neck_pitch.mode = BUMPER_MODE_SPEED;
        motion_threedimrotate_target_p = pitch_ang;
        double now_p = IMU_GetPitch();
        PID_calc(&pid_rotate_pitch, now_p, motion_threedimrotate_target_p);
        remora->bumper_neck_pitch.speed = pid_rotate_pitch.out;
    }

}





/**
 * 负责运动状态的储存与管理
 */
void Motion_Update(fish_t *remora){
    if(motion_mode == MOTION_MODE_THREEDIMROTATE){
        Motion_ThreeDimRotate(motion_threedimrotate_target_y, motion_threedimrotate_target_p, motion_threedimrotate_target_r, remora);
    }
    if (motion_mode == MOTION_MODE_TEST1)
        Motion_SetTest1(remora);
    if (motion_mode == MOTION_MODE_STOP)
        Motion_SetStop(remora);
    if (motion_mode == MOTION_MODE_TEST2)
        Motion_SetTest2(remora, motion_test2_vf, motion_test2_vd, motion_test2_vr, motion_test2_vp, motion_test2_vy);
    if (motion_mode == MOTION_MODE_NECKPITCH)
        Motion_NeckPitch(remora);
    if (motion_mode == MOTION_MODE_PID_ROLL)
        Motion_PidRoll(remora);
    if (motion_mode == MOTION_MODE_DETACH_S1)
        Motion_DetachStep1(remora);
    if (motion_mode == MOTION_MODE_DETACH_S2)
        Motion_DetachStep2(remora);
    if (motion_mode == MOTION_MODE_FRONT)
        Motion_Front(remora);
    if (motion_mode == MOTION_MODE_FRONTPITCH)
        Motion_Front_Pitch(remora);
    if (motion_mode == MOTION_MODE_REMOTE)
        Motion_Remote_Control(remora);

}



